import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, TimerAction, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from xacro import process_file
from launch.actions import SetEnvironmentVariable
import os

def generate_launch_description():
    pkg_openarm = get_package_share_directory('realsense_gazebo_plugin')
    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')

    # -----------------------------
    # 设置 GAZEBO_MODEL_PATH（只加 gazebo_models 目录，而不是整个包）
    # -----------------------------
    gazebo_model_path = os.path.join(pkg_openarm, "share")
    if 'GAZEBO_MODEL_PATH' in os.environ:
        gazebo_model_path = os.pathsep.join([os.environ['GAZEBO_MODEL_PATH'], gazebo_model_path])

    # -----------------------------
    # Gazebo world
    # -----------------------------
    world_path = os.path.join(pkg_gazebo_ros, 'worlds', 'empty.world')
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py')
        ),
        launch_arguments={
            # 'world': world_path,
            'verbose': 'true',
        }.items()
    )

    # -----------------------------
    # Robot description from xacro
    # -----------------------------
    xacro_file = os.path.join(pkg_openarm, 'urdf', 'd435.xacro')
    robot_description_config = process_file(xacro_file).toxml()

    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[{'robot_description': robot_description_config},
                    {'use_sim_time': True}]
    )

    # -----------------------------
    # Spawn entity (延迟 3 秒)
    # -----------------------------
    spawn_entity = TimerAction(
        period=3.0,
        actions=[Node(
            package="gazebo_ros",
            executable="spawn_entity.py",
            arguments=[
                "-topic", "robot_description",
                "-entity", "openarm_bimanual"
            ],
            output="screen"
        )]
    )

    # -----------------------------
    # ros2_control controllers (延迟 8 秒)
    # -----------------------------
    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
        output='screen',
        parameters=[{'use_sim_time': True}]
    )
    left_arm_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["left_joint_trajectory_controller", "--controller-manager", "/controller_manager"],
        output='screen',
        parameters=[{'use_sim_time': True}]
    )
    right_arm_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["right_joint_trajectory_controller", "--controller-manager", "/controller_manager"],
        output='screen',
        parameters=[{'use_sim_time': True}]
    )

    controller_timer = TimerAction(
        period=8.0,
        actions=[
            joint_state_broadcaster_spawner,
            left_arm_spawner,
            right_arm_spawner
        ]
    )

    # -----------------------------
    # LaunchDescription
    # -----------------------------
    return LaunchDescription([
        SetEnvironmentVariable('GAZEBO_MODEL_PATH', gazebo_model_path),
        gazebo,
        robot_state_publisher,
        spawn_entity,
        # controller_timer,
    ])